#ifndef __QUADPOD_H__
#define __QUADPOD_H__
/*
 *****************************************************************************
 *
 * File Name    : quadpod.h
 * Title        : Firmware for QUADPOD robot
 * Author       : Martin Metal
 * Date         : 18/08/2012
 * Revised      : 27/03/2012
 * Version      : 1.0
 * Target MCU   : Atmel AVR ATMega8/ATMega16
 * Editor Tabs  : 3
 * Description  : The central header file for the HEXAPOD project.
 * Remark       : Folding in VIM requires <<<,>>> instead of typical {{{,}}}
 *
 * QUADPOD Kinematics
 * -------------------
 * The construction uses 3DOF construction for arms. The tip of each
 * arm can be set into any arbitrary position [x,y,z], which are limited 
 * by the physical construction of the arm only.
 *
 * A coordinated positioning of all the arm create a gait. This construction
 * use the crawling gait, which is statically balanced. Static balance means
 * that the robot is stable is any phase of the walking movement. Speed is
 * limited while the crawling allows to re-position just one arm at time.
 * Other three arms must rest on the walking surface supporting the 
 * construction. Static stability is achieved when the CoG always remains
 * within a virtual triangle formed by the supportive arms. Additionally,
 * the CoG should have a minimal distance to the nearest edge of this virtual 
 * triangle larger than zero. The closer to zero the less static stability.
 * The higher the stability margin the stable the construction. Classic linear
 * movements of the arms along the direction fulfills the condition of CoG
 * remaining inside the stable region however the stability margin approaches
 * zero in 2 stages of this gait pattern 1-3-2-4. This construction uses
 * tilting of the robot To increase the stability margin. The more tilting
 * the greater stability margin is achieved. Further on lizard-like movement
 * is implemented to provide greater stability margin than provided by
 * tilting function. Lizard is moving its body when it crawls. This robot
 * does not have this flexibility and therefore is imitating this movement 
 * only.
 *
 * This simulation shows the logic used to calculate the movement, having
 * a fixed gait. The gait is implemented as 1-3-2-4, where the
 * topology of moving joints (servos) is shown in the drawing:
 *
 *                       +y
 *                 1 +----|----+ 2
 *                   | Q2 | Q1 |
 *            -x ------- CoG ------- +x
 *                   | Q3 | Q4 |
 *                 4 +----|----+ 3
 *                       -y
 * 
 * CoG = Center of Gravity, axis x and y. The angle gamma start in Q1 and
 * rotates counterclockwise. Gamma is the angle positioning the shoulder
 * pivot.
 *
 * All the coordinates are calculated for the orthogonal x/y system.
 * The orientation of the system is relative to center of gravity of
 * the quadpod. All the shoulder servos are then rotated. The angle
 * between the plane of servo and plane of quadpod's body is depicted
 * in the schematics drawings:
 *
 * NOTE: The project uses the rotation matrix for counterclockwise rotation
 *
 *                [ cos(G) , -sin(G) ]
 *                [ sin(G) ,  cos(G) ]
 *
 *       while we rotate the walking vector and not the servo. 
 *       Therefore the counterclockwise rotation is taken.
 *
 * ARM No.1: angle 45 deg. 
 * (plane of servo must be rotated 45 degrees )
 * (clockwise to meet the x-y plane of the robot)
 * ----------------------
 *          servo +y   +y
 *                \    |    / servo +x
 *                  \  |  /         
 *           -x ______\|/___0____+x 
 *                   / |\   360     
 *                 /   |  \         
 *               /     |    \       
 *                     -y           
 *
 *
 * ARM No.2: 315 deg
 * -------------------
 *                     +y
 *                \    |    / servo +y
 *                  \  |  /
 *           -x ______\|/___0____+x
 *                   / |\   360
 *                 /   |  \
 *               /     |    \
 *                     -y  servo +x
 *
 *
 * ARM No.3: 225 deg
 * -------------------
 *                     +y
 *                \    |    /          
 *                  \  |  /
 *           -x ______\|/___0____+x
 *                   / |\   360
 *                 /   |  \
 *               /     |    \servo +y
 *          servo +x   -y
 *
 *
 * ARM No.4: 135 deg
 * -------------------
 *          servo +x   +y
 *                \    |    /         
 *                  \  |  /
 *           -x ______\|/___0____+x
 *                   / |\   360
 *                 /   |  \
 *               /     |    \
 *          servo +y   -y
 *
 * Next to this definition we define the sweet point for each arm.
 * The sweet point is the SP[x,y,z]. The tip of arm can move within 
 * the circle having the centre at sweet point and radius of step/2.
 * For the given physical construction has sweet point:
 *
 *  [36,36,51] = [51,0,51] = [0,51,51]
 *
 *   generally for each combination of x,y where:
 *                  51=hypot(x,y);
 *
 * The movement is than given by creating a vector V, where x=0, z=0
 * and y in <-step/2;+step/2>. The reference point is SP. This requires
 * to transpose SP into location [0,0,0]. Direction of the movement 
 * is achieved by rotating the vector around the transposed SP. 
 * After rotation, we superpose SP back to it original location at 
 * [36,36,51]. Finally the vector is rotated separately for each arm to
 * achieve the gait. 
 *
 * The robot uses two different schemas in positioning the arms 
 * relatively to the body, CoG respectively. The CRAB and FROG. 
 * Both schemas uses different superposition of the sweet point.
 * The absolute position of the SP in the drawing is indicated 
 * in the drawings:
 *
 *  CRAB: x=+/-51, y=0, z=51
 *  -----
 *
 *  +  o   o  +
 *
 *  +  o   o  +
 *
 *
 * FROG: x=+/-36, y=+/-36, z=51
 * -----
 *
 *  +         +
 *     o   o
 *
 *     o   o
 *  +         +
 *
 * where + - tip of arm
 *       o - shoulder joint (pivot)
 *
 *
 * PHYSICAL CONSTRAINTS
 * ----------------------
 *
 * Define physical dimensions of the robot.
 *
 *    coxa=14.8 mm
 *    femur=45.45 mm
 *    tibia=59.29 mm
 *    base=92 mm
 *
 * Based on this physical dimensions, the maximal step can be 
 * achieved as 102mm, preferably 98mm. Minimal step height is 0, 
 * maximal height is 79mm. Ideal height is 51mm.
 *
 */


#define SIGNALED	  						0x01

#define INF				  					100

#if defined(ADVANCED_SWAY)
#define STEP_TAB_SIZE					0x12 // 18 steps, 
#else
#define STEP_TAB_SIZE					0xC  // 12 steps, 
#endif

#define LINREG_NO_OF_STEPS				0x06 // 14 steps between tabulated entries is calculated (linear regression)

#define STRAIGHT							0x0
#define ROTATE								0x10 // 16 decimal.
// Movemont type constants. Robot can walk straight or rotate on one spot.

#define SERVOSHIELD_DDR					DDRD
#define SERVOSHIELD_CTRL				PORTD
#define MR_BANK_0							PD5
#define MR_BANK_1							PD6
#define CP0									PD7
/* HW Definition for the servo shield. Servo shield constitutes two Johnson
 * decade counter (two banks) with 10 decoded outputs (4017). The application 
 * utilizes 3 control pins:
 *
 *  +---------------------------+-----------+-----------+
 *  |     Name                  |  4017 pin | PIN ATmega|
 *  +---------------------------+-----------+-----------+
 *  | Master Reset (bank 0)     |    MR     |    PD5    |
 *  | Master Reset (bank 1)     |    MR     |    PD6    |
 *  | clock input H->L (common) |   CP1N    |    GND    |
 *  | clock input L->H (common) |   CP0     |    PD7    |
 *  +---------------------------+-----------+-----------+
 *
 * The application does not utilize the CP1N pin. It is permanently connected
 * to low level signal.
 *
 * The counter generates low state on output Q1-Q9 and high status on Q0 after
 * reset. Q0 output is therefore not used in the application. The counter is
 * clocked on the CP0 input.  First rising edge on CP0 brings the output Q1 to
 * high state.  Next clock signal on CP0 sets Q1 to low again and drives Q2
 * high. Signal propagates with every rising edge on CP0 until MR signal is
 * sent. Theoretically one Johnson decade counter can control 9 servos, the
 * implementation reduces this count to 6 servos only. The reason being is the
 * time interval between two control PWM pulses to each servo (20ms).
 *
 * Servo requires a control signal ranging from 544us to 2400us, where 544 us
 * set the servo to the angle position 0 and 2400us to the angle position 180
 * degrees. The 1472us sets the centered position, 90 degrees.  Thus the
 * equation for ideal servo:
 *
 * 		y = 590.87 * x + 544 [us]
 *
 * Typical servos are designed for control pulses ranging from 0.8 through 2.2
 * ms. It means that 0 and 180 degree limits are not achievable. The quadpod
 * design is aware of this limitation and using a corrected equation to
 * determine the angles based on the known control pulses. The experiment 
 * for the used servos (9g TowerPro) yielded the equation in this form:
 *
 * 		Y = 636.4 * X + 452.4  [us]
 *
 */

#define	BANK_MAX_SIZE										6
#define	NEUTRAL												2675
/* Each servo shield bank handles 6 servos only.
 * Neutral position = 90 degrees for 14.7456MHz oscilator
 */

#define	COUNT_OF_ARMS										4
#define	COUNT_OF_SERVOS									12
/* Define the physical parameters of the robot. The dimensions do not contain length
 * of COXA. We consider parameter HEIGHT instead defined as the intersection of the
 * longitudinal axis of FEMUR and rotational axis of the shoulder joint
 * (intersection of COXA with the body). This simplification is possible due to the
 * fact that the angle between COXA and FEMUR is fixed. 
 *
 *
 *               AXIS|
 *                   |         ELBOW
 *                   |         (Beta)
 *                   |           /\
 *                   |    FEMUR /  \
 *          SHOULDER |         /    \
 *                ____________/      \
 *           (Gamma) | COXA  (Alpha)  \
 *                   |      /          \
 *                   |                  \
 *                   |    /              \
 *                   |                    \ TIBIA         
 *                   |  /                  \
 *                   |                      \
 *            _______|/                      \
 *            A      |                         
 *            |      |                          
 *         HEIGHT    |                           
 *            |      |                            
 *            V______|                             
 *
 *
 * The angle Beta is the angle between FEMUR and TIBIA and controls the 'reach' of
 * the arm. It indirectly control also the height of the body above the surface,
 * Alpha is the angle between Coxa and Femur.  The angle gamma is the rotational
 * angle of the shoulder joint. The coordinated change of both Gamma and Beta can
 * achieve to move the tip of the TIBIA along the linear line. The physical
 * dimensions allows to achieve the length of the step up to 90 mm. The mechanical
 * properties of used servos and construction material however limits the length of
 * step to 70mm.
 *
 * Considering the plane of the movement as two dimensional (x,y) one can use the
 * geometric formulas to calculate the angles gamma and beta to bring an arm in
 * given x,y position. We always consider z=0, and the robot achieves best, symmetric
 * results on smooth surfaces only.
 */


enum type_t {
	DISABLE = 0,
	ENABLE
};
typedef enum type_t type_t;
/* Generic type for ON and OFF status.
 */

enum gait_t {
	CRAB = 0,
	FROG
};
typedef enum gait_t gait_t;


enum dir_cntrl_t {
	REVERSE = -1,
	STOP = 0,
	FORWARD = 1
};
typedef enum dir_cntrl_t dir_cntrl_t;
/* Declaring the decrement or increment of the step counter.  Robot crawls
 * forwards, when step counter increments, Robot crawls backwards, when step
 * counter decrements.  Robot turns red for incrementing counter, and turns
 * left for decrementing counter.
 */

enum dir_cntrl_req_t {
	movement_no_change = 0,
	movement_change_start,
	movement_change_finish,
	movement_change_phase_0,
	movement_change_phase_1,
	movement_change_phase_2,
	movement_change_phase_3,
	movement_change_phase_4
};
typedef enum dir_cntrl_req_t dir_cntrl_req_t;
/* Declaring the stages of chnaging the type of movement. Movement is either
 * linear along a given vector or rotational, where the robot just spins around.
 * Swaping the derection required 4 stages, each arm receives its own phase.
 */

struct move_t {
	uint8_t				stepper_previous;
	uint8_t				stepper_current;
	uint8_t		 		movement;
	dir_cntrl_req_t  	cntrl_request;
	dir_cntrl_t	  		cntrl_queue;
	dir_cntrl_t	  		cntrl;
	gait_t				gait;
};
typedef struct move_t move_t;
/* Tracking container for the movement.
 */

struct ocr1a_t {
	uint8_t cntr;
	uint8_t bank;
	uint8_t dirty;
	uint8_t servo_index;
};
typedef struct ocr1a_t ocr1a_t;
/* This structure keeps track on the bank switching.  The servo shield uses two
 * decade counters (4017).  The counter keeps track what servo is currently
 * controlled, and the bank takes care about associated decade counter.  This
 * shield can control up to 16 servos, but one decade counter can drive 9
 * servos at maximum. Therefore two circuits and bank switching among them.
 */ 

struct gait_cntrl_t {
	uint8_t	kinematics_dirty;
	uint8_t	angle_index;
	uint8_t	angle_dirty;
};
typedef struct gait_cntrl_t gait_cntrl_t;
/* Required for gait calculation
 */


struct servo_cntrl_t {
	uint16_t past[12];
	uint16_t current[12];
	uint16_t future[12];
};
typedef struct servo_cntrl_t servo_cntrl_t;
/* This is the latch buffer for servo. It offers three registers for servo
 * data. The steps are tabulated and the distances between tabulated values
 * will be connected by means of linear approximation. The step data then
 * follow linear regression between two tabulated points. The number of steps
 * along the linear regression determines the speed of the movement of the arm.
 */


struct speed_t {
	uint8_t max;
	uint8_t index;
};
typedef struct speed_t speed_t;
/* Defines number of steps along the linear regression calculated for two
 * points in the servo_cntrl_t structure. The total number of steps defines the
 * overall velocity of moving the arms.  The velocity defines how realistic the
 * movements are.
 */


struct arm_t {
	uint16_t		servo_shoulder;
	uint16_t		servo_elbow;
	uint16_t		servo_anckle;
};
typedef struct arm_t arm_t;

struct step_t {
	arm_t	arm[4];
};
typedef struct step_t step_t;

struct run_t {
	step_t step[STEP_TAB_SIZE];
};
typedef struct run_t run_t;
/* System of structures used to describe the movement of the quadpod.  The
 * movement data are calculated outside this project and are just inserted into
 * this container. The design uses 16 tabulated points for each arm. Those 16
 * points are are synchronized between all the arms and servos. The top most
 * structure refines the "run", consists of 16 steps. Each step requires 6
 * arms, two servos each.
 */ 


   /***********************/
   /* FUNCTION PROTOTYPES */
   /***********************/

void servo_shield_port_init(); 
/* HW configuration method. Configured pins PD5 though PD7 as output pins.
 * Those pins then controls the Master Reset (MR) of both used decade counters
 * and clocking them.
 */

void servo_shield_init( void );
/* This command configures the internal, 16-bit timer1 for the CTC mode
 * (compare match). Controller generates compare match interrupt when the
 * internal counter overflows the preset value (OCR1A). This routine also sets
 * the speed of counting by setting the desired prescaler. The prescaler
 * divides the system clock by factor (8,64,256,or 1024). 
 */
 
void servo_shield_cntrl ( type_t );
/* This method enables or disables the interrupt generated by compare match
 * overflow.
 */

void fill_servo_page_P( move_t );
void fill_servo_page( move_t );
/* This function copies one page of the preset movement instructions from EEPROM
 * or PROGMEM into the servo latch buffer. The latch buffer has three stages.
 * One page contain instructions for 12 servos. The whole page is transferred
 * into the first stage of the latch buffer. The copy operation shifts the
 * content of obsoleted first stage into the second stage before overwriting the
 * first stage with new data. The third stage holds the current data for servo
 * movement and is updated by 'fill_servo_buffer()' routine only. 
 */

void determine_next_step( uint8_t, uint8_t );
/* ...
 */

void fill_servo_buffer( move_t, speed_t );
/* Movement function. This function determines the current data for a servo
 * based of the content of the first and second stage of the latch buffer.
 * Technically, the function calculates the linear regression between two
 * tabulated entries. The move_t structure defines the two pointers to the field
 * of tabulated entries for the movement and speed_t brings then data to
 * calculate the linear regression between tabulated points. User can define the
 * granularity of the regression, which in turns translates into smoothness of
 * the movement and speed of the movement. Carefully selected data produces
 * realistic movements of the arms of the robot. 
 */

void init( void );
/* Performs basic initialization of the basic status. 
 */

void irq0_init( void );
/* Initializes the MCU for interrupts from external source - INT0
 */

void irq0_cntrl( type_t mode );
/* Enables/disables interrupt generating by an falling edge on INT0 - pin PD2 (4)
 */

extern void ir_sensor_init(); 
/* Initialize the counter used to IR Decoder.
 */

extern void ir_sensor_cntrl( type_t mode );
/* Enables/disables interrupt called by the compare match 
 * counter/timer0
 */

#ifdef USART_DEBUG
//<<<
void	USART_init();
#endif
//>>>

#endif // __QUADPOD_H__
